#include <stdint.h>
#include <stddef.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <map>
#include <chrono>
#include "Format.h"
#include "Mavlink.h"

int main(int argc, char **argv)
{
    unsigned char szT[280];
    FILE *fp = fopen("heartbeat.dat", "rb");
    if (fp != NULL)
    {
        fread(szT, 280, 1, fp);
        fclose(fp);
    }

    CFormat fmt1, fmt2, fmt3;
    fmt1.load("xml/mavlink.xml");
    fmt2.load("xml/mavlink_heartbeat.xml");
    fmt3.load("xml/mavlink_global_position_int.xml");
    fmt1.decode(szT, 280);
    fmt1.print();

    const std::vector<unsigned char> &v = boost::any_cast<std::vector<unsigned char>>(fmt1.get("payload64"));
    switch (boost::any_cast<unsigned long long>(fmt1.get("msgid")))
    {
    case 0:
    {
        int l = boost::any_cast<unsigned char>(fmt1.get("len")) + 10;
        unsigned char c = szT[l];
        szT[l] = 50;
        unsigned short crc = crc16_mcrf4xx(szT + 1, l);
        if ((crc & 0xff) != c || (crc >> 8) != szT[l + 1])
        {
            printf("heartbeat crc error!\n");
        }
        fmt2.decode(v);
        fmt2.print();
        fmt2.set("custom_mode", (unsigned int)6);
        fmt2.print();

        std::vector<unsigned char> vc;
        fmt2.encode(vc);
        for (int i = 0; i < vc.size(); i++)
        {
            printf("%02X", vc[i]);
            if ((i & 0x07) == 0x07)
            {
                printf(" ");
            }
        }
        printf("\n");

        fmt1.set("payload64", vc);
        fmt1.encode(vc);
        vc[l] = 50;
        crc = crc16_mcrf4xx(vc.data() + 1, l);
        vc[l] = (crc & 0xff);
        vc[l + 1] = (crc >> 8);
        fmt1.print();
        for (int i = 0; i < vc.size(); i++)
        {
            printf("%02X", vc[i]);
            if ((i & 0x07) == 0x07)
            {
                printf(" ");
            }
        }
        printf("\n");
    }
    break;

    default:
        break;
    }

    CMavlink mavlink;
    int msgid = mavlink.decode(szT, 280);
    const CFormat &fmtM = mavlink.get();
    fmtM.print();
    const CFormat &fmtH = mavlink.get(msgid);
    fmtH.print();
    std::vector<unsigned char> vc;
    int n = mavlink.encode(0, vc);
    for (int i = 0; i < n; i++)
    {
        printf("%02X", vc[i]);
        if ((i & 0x07) == 0x07)
        {
            printf(" ");
        }
    }
    printf("\n");

    {
        CMavlink mavlink;
        int msgid = mavlink.decode(vc);
        const CFormat &fmtM = mavlink.get();
        fmtM.print();
        const CFormat &fmtH = mavlink.get(msgid);
        fmtH.print();
    }

    //{
    //    uint16_t crc = 0;
    //    auto ts = std::chrono::system_clock::now();
    //    for (int j = 0; j < 10000; j++)
    //    {
    //        szT[0] = j;
    //        // crc += crc16_mcrf4xx(szT, 4096);
    //    }
    //    auto te = std::chrono::system_clock::now();
    //    std::cout << (double)(te - ts).count() / 1000000 << " ms\t" << (te - ts).count() << " count\t" << crc << std::endl;
    //}
    return 0;
}
